<!DOCTYPE html><html lang="zh-CN" data-theme="light"><head><meta charset="UTF-8"><meta http-equiv="X-UA-Compatible" content="IE=edge"><meta name="viewport" content="width=device-width, initial-scale=1.0,viewport-fit=cover"><title>3D对象识别的假设验证 | 荒岛</title><meta name="author" content="Fi9zero"><meta name="copyright" content="Fi9zero"><meta name="format-detection" content="telephone=no"><meta name="theme-color" content="#ffffff"><meta name="description" content="基于全局假设验证的 3D 物体识别这是一个完整的 3D 物体识别流程，从点云输入到最终验证，实现了对场景中目标物体的鲁棒识别。  1. 算法流程总览该程序实现了一个经典的 3D 物体识别 Pipeline：  数据加载：加载模型和场景点云。 预处理：清理 NaN 点。 特征提取： 法向量估计 均匀采样关键点 SHOT 描述子计算   建立对应：通过 KdTree 匹配描述子。 姿态聚类：使用 Ho">
<meta property="og:type" content="article">
<meta property="og:title" content="3D对象识别的假设验证">
<meta property="og:url" content="https://ckyfi9zero.github.io/2025/08/03/2025-08-03-3D%E5%AF%B9%E8%B1%A1%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81/index.html">
<meta property="og:site_name" content="荒岛">
<meta property="og:description" content="基于全局假设验证的 3D 物体识别这是一个完整的 3D 物体识别流程，从点云输入到最终验证，实现了对场景中目标物体的鲁棒识别。  1. 算法流程总览该程序实现了一个经典的 3D 物体识别 Pipeline：  数据加载：加载模型和场景点云。 预处理：清理 NaN 点。 特征提取： 法向量估计 均匀采样关键点 SHOT 描述子计算   建立对应：通过 KdTree 匹配描述子。 姿态聚类：使用 Ho">
<meta property="og:locale" content="zh_CN">
<meta property="og:image" content="https://ckyfi9zero.github.io/image/avatar.jpg">
<meta property="article:published_time" content="2025-08-03T13:12:53.000Z">
<meta property="article:modified_time" content="2025-08-03T13:36:58.711Z">
<meta property="article:author" content="Fi9zero">
<meta property="article:tag" content="c++">
<meta property="article:tag" content="PCL">
<meta name="twitter:card" content="summary">
<meta name="twitter:image" content="https://ckyfi9zero.github.io/image/avatar.jpg"><script type="application/ld+json">{
  "@context": "https://schema.org",
  "@type": "BlogPosting",
  "headline": "3D对象识别的假设验证",
  "url": "https://ckyfi9zero.github.io/2025/08/03/2025-08-03-3D%E5%AF%B9%E8%B1%A1%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81/",
  "image": "https://ckyfi9zero.github.io/image/avatar.jpg",
  "datePublished": "2025-08-03T13:12:53.000Z",
  "dateModified": "2025-08-03T13:36:58.711Z",
  "author": [
    {
      "@type": "Person",
      "name": "Fi9zero",
      "url": "https://ckyfi9zero.github.io"
    }
  ]
}</script><link rel="shortcut icon" href="/image/favicon.ico"><link rel="canonical" href="https://ckyfi9zero.github.io/2025/08/03/2025-08-03-3D%E5%AF%B9%E8%B1%A1%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81/index.html"><link rel="preconnect" href="//cdn.jsdelivr.net"/><link rel="preconnect" href="//busuanzi.ibruce.info"/><link rel="stylesheet" href="/css/index.css"><link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/@fortawesome/fontawesome-free/css/all.min.css"><script>
    (() => {
      
    const saveToLocal = {
      set: (key, value, ttl) => {
        if (!ttl) return
        const expiry = Date.now() + ttl * 86400000
        localStorage.setItem(key, JSON.stringify({ value, expiry }))
      },
      get: key => {
        const itemStr = localStorage.getItem(key)
        if (!itemStr) return undefined
        const { value, expiry } = JSON.parse(itemStr)
        if (Date.now() > expiry) {
          localStorage.removeItem(key)
          return undefined
        }
        return value
      }
    }

    window.btf = {
      saveToLocal,
      getScript: (url, attr = {}) => new Promise((resolve, reject) => {
        const script = document.createElement('script')
        script.src = url
        script.async = true
        Object.entries(attr).forEach(([key, val]) => script.setAttribute(key, val))
        script.onload = script.onreadystatechange = () => {
          if (!script.readyState || /loaded|complete/.test(script.readyState)) resolve()
        }
        script.onerror = reject
        document.head.appendChild(script)
      }),
      getCSS: (url, id) => new Promise((resolve, reject) => {
        const link = document.createElement('link')
        link.rel = 'stylesheet'
        link.href = url
        if (id) link.id = id
        link.onload = link.onreadystatechange = () => {
          if (!link.readyState || /loaded|complete/.test(link.readyState)) resolve()
        }
        link.onerror = reject
        document.head.appendChild(link)
      }),
      addGlobalFn: (key, fn, name = false, parent = window) => {
        if (!false && key.startsWith('pjax')) return
        const globalFn = parent.globalFn || {}
        globalFn[key] = globalFn[key] || {}
        globalFn[key][name || Object.keys(globalFn[key]).length] = fn
        parent.globalFn = globalFn
      }
    }
  
      
      const activateDarkMode = () => {
        document.documentElement.setAttribute('data-theme', 'dark')
        if (document.querySelector('meta[name="theme-color"]') !== null) {
          document.querySelector('meta[name="theme-color"]').setAttribute('content', '#0d0d0d')
        }
      }
      const activateLightMode = () => {
        document.documentElement.setAttribute('data-theme', 'light')
        if (document.querySelector('meta[name="theme-color"]') !== null) {
          document.querySelector('meta[name="theme-color"]').setAttribute('content', '#ffffff')
        }
      }

      btf.activateDarkMode = activateDarkMode
      btf.activateLightMode = activateLightMode

      const theme = saveToLocal.get('theme')
    
          theme === 'dark' ? activateDarkMode() : theme === 'light' ? activateLightMode() : null
        
      
      const asideStatus = saveToLocal.get('aside-status')
      if (asideStatus !== undefined) {
        document.documentElement.classList.toggle('hide-aside', asideStatus === 'hide')
      }
    
      
    const detectApple = () => {
      if (/iPad|iPhone|iPod|Macintosh/.test(navigator.userAgent)) {
        document.documentElement.classList.add('apple')
      }
    }
    detectApple()
  
    })()
  </script><script>const GLOBAL_CONFIG = {
  root: '/',
  algolia: undefined,
  localSearch: {"path":"/search.xml","preload":true,"top_n_per_article":1,"unescape":false,"languages":{"hits_empty":"未找到符合您查询的内容：${query}","hits_stats":"共找到 ${hits} 篇文章"}},
  translate: undefined,
  highlight: {"plugin":"highlight.js","highlightCopy":true,"highlightLang":true,"highlightHeightLimit":false,"highlightFullpage":false,"highlightMacStyle":true},
  copy: {
    success: '复制成功',
    error: '复制失败',
    noSupport: '浏览器不支持'
  },
  relativeDate: {
    homepage: false,
    post: false
  },
  runtime: '',
  dateSuffix: {
    just: '刚刚',
    min: '分钟前',
    hour: '小时前',
    day: '天前',
    month: '个月前'
  },
  copyright: undefined,
  lightbox: 'null',
  Snackbar: undefined,
  infinitegrid: {
    js: 'https://cdn.jsdelivr.net/npm/@egjs/infinitegrid/dist/infinitegrid.min.js',
    buttonText: '加载更多'
  },
  isPhotoFigcaption: false,
  islazyloadPlugin: false,
  isAnchor: false,
  percent: {
    toc: true,
    rightside: false,
  },
  autoDarkmode: false
}</script><script id="config-diff">var GLOBAL_CONFIG_SITE = {
  title: '3D对象识别的假设验证',
  isHighlightShrink: false,
  isToc: true,
  pageType: 'post'
}</script><link rel="stylesheet" href="/css/custom.css"><link rel="stylesheet" href="/css/pig.css"><link rel="stylesheet" href="https://npm.elemecdn.com/ethan4116-blog/lib/css/plane_v2.css"><meta name="generator" content="Hexo 7.3.0"></head><body><div id="sidebar"><div id="menu-mask"></div><div id="sidebar-menus"><div class="avatar-img text-center"><img src="/image/avatar.jpg" onerror="this.onerror=null;this.src='/img/friend_404.gif'" alt="avatar"/></div><div class="site-data text-center"><a href="/archives/"><div class="headline">文章</div><div class="length-num">54</div></a><a href="/tags/"><div class="headline">标签</div><div class="length-num">15</div></a><a href="/categories/"><div class="headline">分类</div><div class="length-num">8</div></a></div><div class="menus_items"><div class="menus_item"><a class="site-page" href="/"><i class="fa-fw fas fa-home"></i><span> 首页</span></a></div><div class="menus_item"><a class="site-page" href="/archives/"><i class="fa-fw fas fa-archive"></i><span> 时间轴</span></a></div><div class="menus_item"><a class="site-page" href="/tags/"><i class="fa-fw fas fa-tags"></i><span> 标签</span></a></div><div class="menus_item"><a class="site-page" href="/categories/"><i class="fa-fw fas fa-folder-open"></i><span> 分类</span></a></div><div class="menus_item"><a class="site-page" href="/booklist/"><i class="fa-fw fas fa-book"></i><span> 书单</span></a></div><div class="menus_item"><a class="site-page" href="/link/"><i class="fa-fw fas fa-link"></i><span> 链接</span></a></div><div class="menus_item"><a class="site-page" href="/about/"><i class="fa-fw fas fa-heart"></i><span> 关于</span></a></div></div></div></div><div class="post" id="body-wrap"><header class="post-bg" id="page-header" style="background-image: url(/image/per%20page.jpg);"><nav id="nav"><span id="blog-info"><a class="nav-site-title" href="/"><span class="site-name">荒岛</span></a><a class="nav-page-title" href="/"><span class="site-name">3D对象识别的假设验证</span><span class="site-name"><i class="fa-solid fa-circle-arrow-left"></i><span>  返回首页</span></span></a></span><div id="menus"><div id="search-button"><span class="site-page social-icon search"><i class="fas fa-search fa-fw"></i><span> 搜索</span></span></div><div class="menus_items"><div class="menus_item"><a class="site-page" href="/"><i class="fa-fw fas fa-home"></i><span> 首页</span></a></div><div class="menus_item"><a class="site-page" href="/archives/"><i class="fa-fw fas fa-archive"></i><span> 时间轴</span></a></div><div class="menus_item"><a class="site-page" href="/tags/"><i class="fa-fw fas fa-tags"></i><span> 标签</span></a></div><div class="menus_item"><a class="site-page" href="/categories/"><i class="fa-fw fas fa-folder-open"></i><span> 分类</span></a></div><div class="menus_item"><a class="site-page" href="/booklist/"><i class="fa-fw fas fa-book"></i><span> 书单</span></a></div><div class="menus_item"><a class="site-page" href="/link/"><i class="fa-fw fas fa-link"></i><span> 链接</span></a></div><div class="menus_item"><a class="site-page" href="/about/"><i class="fa-fw fas fa-heart"></i><span> 关于</span></a></div></div><div id="toggle-menu"><span class="site-page"><i class="fas fa-bars fa-fw"></i></span></div></div></nav><div id="post-info"><h1 class="post-title">3D对象识别的假设验证</h1><div id="post-meta"><div class="meta-firstline"><span class="post-meta-date"><i class="far fa-calendar-alt fa-fw post-meta-icon"></i><span class="post-meta-label">发表于</span><time class="post-meta-date-created" datetime="2025-08-03T13:12:53.000Z" title="发表于 2025-08-03 21:12:53">2025-08-03</time><span class="post-meta-separator">|</span><i class="fas fa-history fa-fw post-meta-icon"></i><span class="post-meta-label">更新于</span><time class="post-meta-date-updated" datetime="2025-08-03T13:36:58.711Z" title="更新于 2025-08-03 21:36:58">2025-08-03</time></span><span class="post-meta-categories"><span class="post-meta-separator">|</span><i class="fas fa-inbox fa-fw post-meta-icon"></i><a class="post-meta-categories" href="/categories/PCL/">PCL</a></span></div><div class="meta-secondline"><span class="post-meta-separator">|</span><span class="post-meta-pv-cv" id="" data-flag-title=""><i class="far fa-eye fa-fw post-meta-icon"></i><span class="post-meta-label">浏览量:</span><span id="busuanzi_value_page_pv"><i class="fa-solid fa-spinner fa-spin"></i></span></span></div></div></div></header><main class="layout" id="content-inner"><div id="post"><article class="container post-content" id="article-container"><h1 id="基于全局假设验证的-3D-物体识别"><a href="#基于全局假设验证的-3D-物体识别" class="headerlink" title="基于全局假设验证的 3D 物体识别"></a>基于全局假设验证的 3D 物体识别</h1><p>这是一个完整的 3D 物体识别流程，从点云输入到最终验证，实现了对场景中目标物体的鲁棒识别。</p>
<hr>
<h2 id="1-算法流程总览"><a href="#1-算法流程总览" class="headerlink" title="1. 算法流程总览"></a>1. 算法流程总览</h2><p>该程序实现了一个经典的 3D 物体识别 Pipeline：</p>
<ol>
<li><strong>数据加载</strong>：加载模型和场景点云。</li>
<li><strong>预处理</strong>：清理 NaN 点。</li>
<li><strong>特征提取</strong>：<ul>
<li>法向量估计</li>
<li>均匀采样关键点</li>
<li>SHOT 描述子计算</li>
</ul>
</li>
<li><strong>建立对应</strong>：通过 KdTree 匹配描述子。</li>
<li><strong>姿态聚类</strong>：使用 Hough 或 GC 算法生成初始姿态假设。</li>
<li><strong>精配准</strong>：使用 ICP 对每个假设进行精配准。</li>
<li><strong>假设验证</strong>：使用 <code>GlobalHypothesesVerification</code> 过滤错误假设。</li>
<li><strong>可视化</strong>：显示最终结果。</li>
</ol>
<hr>
<h2 id="2-核心组件详解"><a href="#2-核心组件详解" class="headerlink" title="2. 核心组件详解"></a>2. 核心组件详解</h2><h3 id="2-1-预处理：removeNaNFromPointCloud"><a href="#2-1-预处理：removeNaNFromPointCloud" class="headerlink" title="2.1 预处理：removeNaNFromPointCloud"></a>2.1 预处理：<code>removeNaNFromPointCloud</code></h3><p><strong>问题</strong>：原始点云常包含 <code>(NaN, NaN, NaN)</code> 的无效点。<br><strong>解决方案</strong>：在所有处理前，使用 <code>pcl::removeNaNFromPointCloud</code> 清理 <code>model</code> 和 <code>scene</code> 点云，得到 <code>model_clean</code> 和 <code>scene_clean</code>。后续所有操作都基于清理后的点云。</p>
<h3 id="2-2-关键点提取：UniformSampling"><a href="#2-2-关键点提取：UniformSampling" class="headerlink" title="2.2 关键点提取：UniformSampling"></a>2.2 关键点提取：<code>UniformSampling</code></h3><p><strong>目的</strong>：降低计算量，避免在所有点上计算描述子。<br><strong>方法</strong>：使用 <code>pcl::UniformSampling</code> 对 <code>model_clean</code> 和 <code>scene_clean</code> 进行下采样。<br><strong>关键</strong>：直接调用 <code>filter(*output_cloud)</code>，无需手动管理索引。</p>
<h3 id="2-3-特征描述：SHOTEstimationOMP"><a href="#2-3-特征描述：SHOTEstimationOMP" class="headerlink" title="2.3 特征描述：SHOTEstimationOMP"></a>2.3 特征描述：<code>SHOTEstimationOMP</code></h3><p><strong>描述子</strong>：<code>SHOT</code> (Signature of Histograms of OrienTations) 是一种结合了法向量方向和空间分布的描述子。<br><strong>参数</strong>：</p>
<ul>
<li><code>setRadiusSearch(descr_rad_)</code>：描述子的局部支持半径。</li>
<li><code>setInputCloud()</code>：输入关键点。</li>
<li><code>setInputNormals()</code>：输入关键点的法向量。</li>
<li><code>setSearchSurface()</code>：<strong>必须是清理后的点云</strong> (<code>model_clean</code>&#x2F;<code>scene_clean</code>)，否则会因大小不匹配而失败。</li>
</ul>
<h3 id="2-4-姿态聚类：Hough3DGrouping-vs-GeometricConsistencyGrouping"><a href="#2-4-姿态聚类：Hough3DGrouping-vs-GeometricConsistencyGrouping" class="headerlink" title="2.4 姿态聚类：Hough3DGrouping vs GeometricConsistencyGrouping"></a>2.4 姿态聚类：<code>Hough3DGrouping</code> vs <code>GeometricConsistencyGrouping</code></h3><table>
<thead>
<tr>
<th>算法</th>
<th>原理</th>
<th>特点</th>
</tr>
</thead>
<tbody><tr>
<td><strong>Hough 3D</strong></td>
<td>基于参考帧 (如 BOARD)，将姿态投票到 Hough 空间</td>
<td>对噪声和遮挡鲁棒，但依赖参考帧质量</td>
</tr>
<tr>
<td><strong>GC (Geometric Consistency)</strong></td>
<td>检查对应点对之间的距离是否在所有假设下保持一致</td>
<td>不需要参考帧，计算简单</td>
</tr>
</tbody></table>
<h3 id="2-5-精配准：IterativeClosestPoint-ICP"><a href="#2-5-精配准：IterativeClosestPoint-ICP" class="headerlink" title="2.5 精配准：IterativeClosestPoint (ICP)"></a>2.5 精配准：<code>IterativeClosestPoint (ICP)</code></h3><p><strong>目的</strong>：将由聚类得到的粗略姿态假设进行精化。<br><strong>输入</strong>：由 <code>rototranslations</code> 变换后的 <code>model</code> 点云 和 <code>scene</code> 点云。<br><strong>输出</strong>：更精确的配准结果。</p>
<h3 id="2-6-假设验证：GlobalHypothesesVerification"><a href="#2-6-假设验证：GlobalHypothesesVerification" class="headerlink" title="2.6 假设验证：GlobalHypothesesVerification"></a>2.6 假设验证：<code>GlobalHypothesesVerification</code></h3><p><strong>目的</strong>：过滤由聚类产生的错误假设（误检）。<br><strong>原理</strong>：检查每个假设是否与场景一致，考虑遮挡和杂乱。<br><strong>输出</strong>：一个 <code>std::vector&lt;bool&gt;</code> 掩码，<code>true</code> 表示该假设通过验证。</p>
<hr>
<h2 id="3-关键修复点"><a href="#3-关键修复点" class="headerlink" title="3. 关键修复点"></a>3. 关键修复点</h2><table>
<thead>
<tr>
<th>问题</th>
<th>修复方案</th>
</tr>
</thead>
<tbody><tr>
<td><code>uniform_sampling.compute</code> 报错</td>
<td>改用 <code>filter(*output_cloud)</code></td>
</tr>
<tr>
<td><code>Failed to find match for field &#39;rgba&#39;</code></td>
<td>警告，可忽略</td>
</tr>
<tr>
<td><code>Assertion &#39;isFinite (query)&#39; failed</code></td>
<td>在计算法向量前使用 <code>removeNaNFromPointCloud</code> 清理点云</td>
</tr>
<tr>
<td><code>The number of points ... differs</code></td>
<td><strong>所有</strong>相关操作（<code>setInputCloud</code>, <code>setSearchSurface</code>）都必须使用清理后的点云 (<code>*_clean</code>)</td>
</tr>
</tbody></table>
<hr>
<h2 id="4-可视化内容"><a href="#4-可视化内容" class="headerlink" title="4. 可视化内容"></a>4. 可视化内容</h2><ul>
<li><strong>场景点云</strong>：原始场景，白色。</li>
<li><strong>模型点云</strong>：位于场景左侧，白色。</li>
<li><strong>关键点</strong>：模型和场景的关键点，紫色。</li>
<li><strong>识别实例</strong>：<ul>
<li><strong>未配准</strong>：红色，表示由聚类得到的初始假设。</li>
<li><strong>已配准</strong>：绿色（通过验证）或青色（未通过验证）。</li>
</ul>
</li>
</ul>
<hr>
<h2 id="5-运行命令"><a href="#5-运行命令" class="headerlink" title="5. 运行命令"></a>5. 运行命令</h2><figure class="highlight bash"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br></pre></td><td class="code"><pre><span class="line">./global_hypothesis_verification model.pcd scene.pcd [Options]</span><br><span class="line"></span><br><span class="line"><span class="comment"># 示例</span></span><br><span class="line">./global_hypothesis_verification ../milk.pcd ../milk_cartoon.pcd --algorithm GC -k</span><br></pre></td></tr></table></figure>

<h2 id="代码实现"><a href="#代码实现" class="headerlink" title="代码实现"></a>代码实现</h2><figure class="highlight c++"><table><tr><td class="gutter"><pre><span class="line">1</span><br><span class="line">2</span><br><span class="line">3</span><br><span class="line">4</span><br><span class="line">5</span><br><span class="line">6</span><br><span class="line">7</span><br><span class="line">8</span><br><span class="line">9</span><br><span class="line">10</span><br><span class="line">11</span><br><span class="line">12</span><br><span class="line">13</span><br><span class="line">14</span><br><span class="line">15</span><br><span class="line">16</span><br><span class="line">17</span><br><span class="line">18</span><br><span class="line">19</span><br><span class="line">20</span><br><span class="line">21</span><br><span class="line">22</span><br><span class="line">23</span><br><span class="line">24</span><br><span class="line">25</span><br><span class="line">26</span><br><span class="line">27</span><br><span class="line">28</span><br><span class="line">29</span><br><span class="line">30</span><br><span class="line">31</span><br><span class="line">32</span><br><span class="line">33</span><br><span class="line">34</span><br><span class="line">35</span><br><span class="line">36</span><br><span class="line">37</span><br><span class="line">38</span><br><span class="line">39</span><br><span class="line">40</span><br><span class="line">41</span><br><span class="line">42</span><br><span class="line">43</span><br><span class="line">44</span><br><span class="line">45</span><br><span class="line">46</span><br><span class="line">47</span><br><span class="line">48</span><br><span class="line">49</span><br><span class="line">50</span><br><span class="line">51</span><br><span class="line">52</span><br><span class="line">53</span><br><span class="line">54</span><br><span class="line">55</span><br><span class="line">56</span><br><span class="line">57</span><br><span class="line">58</span><br><span class="line">59</span><br><span class="line">60</span><br><span class="line">61</span><br><span class="line">62</span><br><span class="line">63</span><br><span class="line">64</span><br><span class="line">65</span><br><span class="line">66</span><br><span class="line">67</span><br><span class="line">68</span><br><span class="line">69</span><br><span class="line">70</span><br><span class="line">71</span><br><span class="line">72</span><br><span class="line">73</span><br><span class="line">74</span><br><span class="line">75</span><br><span class="line">76</span><br><span class="line">77</span><br><span class="line">78</span><br><span class="line">79</span><br><span class="line">80</span><br><span class="line">81</span><br><span class="line">82</span><br><span class="line">83</span><br><span class="line">84</span><br><span class="line">85</span><br><span class="line">86</span><br><span class="line">87</span><br><span class="line">88</span><br><span class="line">89</span><br><span class="line">90</span><br><span class="line">91</span><br><span class="line">92</span><br><span class="line">93</span><br><span class="line">94</span><br><span class="line">95</span><br><span class="line">96</span><br><span class="line">97</span><br><span class="line">98</span><br><span class="line">99</span><br><span class="line">100</span><br><span class="line">101</span><br><span class="line">102</span><br><span class="line">103</span><br><span class="line">104</span><br><span class="line">105</span><br><span class="line">106</span><br><span class="line">107</span><br><span class="line">108</span><br><span class="line">109</span><br><span class="line">110</span><br><span class="line">111</span><br><span class="line">112</span><br><span class="line">113</span><br><span class="line">114</span><br><span class="line">115</span><br><span class="line">116</span><br><span class="line">117</span><br><span class="line">118</span><br><span class="line">119</span><br><span class="line">120</span><br><span class="line">121</span><br><span class="line">122</span><br><span class="line">123</span><br><span class="line">124</span><br><span class="line">125</span><br><span class="line">126</span><br><span class="line">127</span><br><span class="line">128</span><br><span class="line">129</span><br><span class="line">130</span><br><span class="line">131</span><br><span class="line">132</span><br><span class="line">133</span><br><span class="line">134</span><br><span class="line">135</span><br><span class="line">136</span><br><span class="line">137</span><br><span class="line">138</span><br><span class="line">139</span><br><span class="line">140</span><br><span class="line">141</span><br><span class="line">142</span><br><span class="line">143</span><br><span class="line">144</span><br><span class="line">145</span><br><span class="line">146</span><br><span class="line">147</span><br><span class="line">148</span><br><span class="line">149</span><br><span class="line">150</span><br><span class="line">151</span><br><span class="line">152</span><br><span class="line">153</span><br><span class="line">154</span><br><span class="line">155</span><br><span class="line">156</span><br><span class="line">157</span><br><span class="line">158</span><br><span class="line">159</span><br><span class="line">160</span><br><span class="line">161</span><br><span class="line">162</span><br><span class="line">163</span><br><span class="line">164</span><br><span class="line">165</span><br><span class="line">166</span><br><span class="line">167</span><br><span class="line">168</span><br><span class="line">169</span><br><span class="line">170</span><br><span class="line">171</span><br><span class="line">172</span><br><span class="line">173</span><br><span class="line">174</span><br><span class="line">175</span><br><span class="line">176</span><br><span class="line">177</span><br><span class="line">178</span><br><span class="line">179</span><br><span class="line">180</span><br><span class="line">181</span><br><span class="line">182</span><br><span class="line">183</span><br><span class="line">184</span><br><span class="line">185</span><br><span class="line">186</span><br><span class="line">187</span><br><span class="line">188</span><br><span class="line">189</span><br><span class="line">190</span><br><span class="line">191</span><br><span class="line">192</span><br><span class="line">193</span><br><span class="line">194</span><br><span class="line">195</span><br><span class="line">196</span><br><span class="line">197</span><br><span class="line">198</span><br><span class="line">199</span><br><span class="line">200</span><br><span class="line">201</span><br><span class="line">202</span><br><span class="line">203</span><br><span class="line">204</span><br><span class="line">205</span><br><span class="line">206</span><br><span class="line">207</span><br><span class="line">208</span><br><span class="line">209</span><br><span class="line">210</span><br><span class="line">211</span><br><span class="line">212</span><br><span class="line">213</span><br><span class="line">214</span><br><span class="line">215</span><br><span class="line">216</span><br><span class="line">217</span><br><span class="line">218</span><br><span class="line">219</span><br><span class="line">220</span><br><span class="line">221</span><br><span class="line">222</span><br><span class="line">223</span><br><span class="line">224</span><br><span class="line">225</span><br><span class="line">226</span><br><span class="line">227</span><br><span class="line">228</span><br><span class="line">229</span><br><span class="line">230</span><br><span class="line">231</span><br><span class="line">232</span><br><span class="line">233</span><br><span class="line">234</span><br><span class="line">235</span><br><span class="line">236</span><br><span class="line">237</span><br><span class="line">238</span><br><span class="line">239</span><br><span class="line">240</span><br><span class="line">241</span><br><span class="line">242</span><br><span class="line">243</span><br><span class="line">244</span><br><span class="line">245</span><br><span class="line">246</span><br><span class="line">247</span><br><span class="line">248</span><br><span class="line">249</span><br><span class="line">250</span><br><span class="line">251</span><br><span class="line">252</span><br><span class="line">253</span><br><span class="line">254</span><br><span class="line">255</span><br><span class="line">256</span><br><span class="line">257</span><br><span class="line">258</span><br><span class="line">259</span><br><span class="line">260</span><br><span class="line">261</span><br><span class="line">262</span><br><span class="line">263</span><br><span class="line">264</span><br><span class="line">265</span><br><span class="line">266</span><br><span class="line">267</span><br><span class="line">268</span><br><span class="line">269</span><br><span class="line">270</span><br><span class="line">271</span><br><span class="line">272</span><br><span class="line">273</span><br><span class="line">274</span><br><span class="line">275</span><br><span class="line">276</span><br><span class="line">277</span><br><span class="line">278</span><br><span class="line">279</span><br><span class="line">280</span><br><span class="line">281</span><br><span class="line">282</span><br><span class="line">283</span><br><span class="line">284</span><br><span class="line">285</span><br><span class="line">286</span><br><span class="line">287</span><br><span class="line">288</span><br><span class="line">289</span><br><span class="line">290</span><br><span class="line">291</span><br><span class="line">292</span><br><span class="line">293</span><br><span class="line">294</span><br><span class="line">295</span><br><span class="line">296</span><br><span class="line">297</span><br><span class="line">298</span><br><span class="line">299</span><br><span class="line">300</span><br><span class="line">301</span><br><span class="line">302</span><br><span class="line">303</span><br><span class="line">304</span><br><span class="line">305</span><br><span class="line">306</span><br><span class="line">307</span><br><span class="line">308</span><br><span class="line">309</span><br><span class="line">310</span><br><span class="line">311</span><br><span class="line">312</span><br><span class="line">313</span><br><span class="line">314</span><br><span class="line">315</span><br><span class="line">316</span><br><span class="line">317</span><br><span class="line">318</span><br><span class="line">319</span><br><span class="line">320</span><br><span class="line">321</span><br><span class="line">322</span><br><span class="line">323</span><br><span class="line">324</span><br><span class="line">325</span><br><span class="line">326</span><br><span class="line">327</span><br><span class="line">328</span><br><span class="line">329</span><br><span class="line">330</span><br><span class="line">331</span><br><span class="line">332</span><br><span class="line">333</span><br><span class="line">334</span><br><span class="line">335</span><br><span class="line">336</span><br><span class="line">337</span><br><span class="line">338</span><br><span class="line">339</span><br><span class="line">340</span><br><span class="line">341</span><br><span class="line">342</span><br><span class="line">343</span><br><span class="line">344</span><br><span class="line">345</span><br><span class="line">346</span><br><span class="line">347</span><br><span class="line">348</span><br><span class="line">349</span><br><span class="line">350</span><br><span class="line">351</span><br><span class="line">352</span><br><span class="line">353</span><br><span class="line">354</span><br><span class="line">355</span><br><span class="line">356</span><br><span class="line">357</span><br><span class="line">358</span><br><span class="line">359</span><br><span class="line">360</span><br><span class="line">361</span><br><span class="line">362</span><br><span class="line">363</span><br><span class="line">364</span><br><span class="line">365</span><br><span class="line">366</span><br><span class="line">367</span><br><span class="line">368</span><br><span class="line">369</span><br><span class="line">370</span><br><span class="line">371</span><br><span class="line">372</span><br><span class="line">373</span><br><span class="line">374</span><br><span class="line">375</span><br><span class="line">376</span><br><span class="line">377</span><br><span class="line">378</span><br><span class="line">379</span><br><span class="line">380</span><br><span class="line">381</span><br><span class="line">382</span><br><span class="line">383</span><br><span class="line">384</span><br><span class="line">385</span><br><span class="line">386</span><br><span class="line">387</span><br><span class="line">388</span><br><span class="line">389</span><br><span class="line">390</span><br><span class="line">391</span><br><span class="line">392</span><br><span class="line">393</span><br><span class="line">394</span><br><span class="line">395</span><br><span class="line">396</span><br><span class="line">397</span><br><span class="line">398</span><br><span class="line">399</span><br><span class="line">400</span><br><span class="line">401</span><br><span class="line">402</span><br><span class="line">403</span><br><span class="line">404</span><br><span class="line">405</span><br><span class="line">406</span><br><span class="line">407</span><br><span class="line">408</span><br><span class="line">409</span><br><span class="line">410</span><br><span class="line">411</span><br><span class="line">412</span><br><span class="line">413</span><br><span class="line">414</span><br><span class="line">415</span><br><span class="line">416</span><br><span class="line">417</span><br><span class="line">418</span><br><span class="line">419</span><br><span class="line">420</span><br><span class="line">421</span><br><span class="line">422</span><br><span class="line">423</span><br><span class="line">424</span><br><span class="line">425</span><br><span class="line">426</span><br><span class="line">427</span><br><span class="line">428</span><br><span class="line">429</span><br><span class="line">430</span><br><span class="line">431</span><br><span class="line">432</span><br><span class="line">433</span><br><span class="line">434</span><br><span class="line">435</span><br><span class="line">436</span><br><span class="line">437</span><br><span class="line">438</span><br><span class="line">439</span><br><span class="line">440</span><br><span class="line">441</span><br><span class="line">442</span><br><span class="line">443</span><br><span class="line">444</span><br><span class="line">445</span><br><span class="line">446</span><br><span class="line">447</span><br><span class="line">448</span><br><span class="line">449</span><br><span class="line">450</span><br><span class="line">451</span><br><span class="line">452</span><br><span class="line">453</span><br><span class="line">454</span><br></pre></td><td class="code"><pre><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/io/pcd_io.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/point_cloud.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/correspondence.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/features/normal_3d_omp.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/features/shot_omp.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/features/board.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/keypoints/uniform_sampling.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/recognition/cg/hough_3d.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/recognition/cg/geometric_consistency.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/recognition/hv/hv_go.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/registration/icp.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/visualization/pcl_visualizer.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/kdtree/kdtree_flann.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/common/transforms.h&gt;</span> </span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;pcl/console/parse.h&gt;</span></span></span><br><span class="line"><span class="meta">#<span class="keyword">include</span> <span class="string">&lt;iostream&gt;</span> <span class="comment">// 添加 iostream 以确保 cout 可用</span></span></span><br><span class="line"></span><br><span class="line"><span class="comment">// 使用标准命名空间</span></span><br><span class="line"><span class="keyword">using</span> <span class="keyword">namespace</span> std;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 定义类型别名</span></span><br><span class="line"><span class="keyword">typedef</span> pcl::PointXYZRGBA PointType;</span><br><span class="line"><span class="keyword">typedef</span> pcl::Normal NormalType;</span><br><span class="line"><span class="keyword">typedef</span> pcl::ReferenceFrame RFType;</span><br><span class="line"><span class="keyword">typedef</span> pcl::SHOT352 DescriptorType;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 点云样式结构体</span></span><br><span class="line"><span class="keyword">struct</span> <span class="title class_">CloudStyle</span></span><br><span class="line">&#123;</span><br><span class="line">    <span class="type">double</span> r;</span><br><span class="line">    <span class="type">double</span> g;</span><br><span class="line">    <span class="type">double</span> b;</span><br><span class="line">    <span class="type">double</span> size;</span><br><span class="line">    <span class="built_in">CloudStyle</span> (<span class="type">double</span> r, <span class="type">double</span> g, <span class="type">double</span> b, <span class="type">double</span> size) : <span class="built_in">r</span>(r), <span class="built_in">g</span>(g), <span class="built_in">b</span>(b), <span class="built_in">size</span>(size) &#123;&#125;</span><br><span class="line">&#125;;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 预定义样式</span></span><br><span class="line"><span class="function">CloudStyle <span class="title">style_white</span> <span class="params">(<span class="number">255.0</span>, <span class="number">255.0</span>, <span class="number">255.0</span>, <span class="number">4.0</span>)</span></span>;</span><br><span class="line"><span class="function">CloudStyle <span class="title">style_red</span> <span class="params">(<span class="number">255.0</span>, <span class="number">0.0</span>, <span class="number">0.0</span>, <span class="number">3.0</span>)</span></span>;</span><br><span class="line"><span class="function">CloudStyle <span class="title">style_green</span> <span class="params">(<span class="number">0.0</span>, <span class="number">255.0</span>, <span class="number">0.0</span>, <span class="number">5.0</span>)</span></span>;</span><br><span class="line"><span class="function">CloudStyle <span class="title">style_cyan</span> <span class="params">(<span class="number">93.0</span>, <span class="number">200.0</span>, <span class="number">217.0</span>, <span class="number">4.0</span>)</span></span>;</span><br><span class="line"><span class="function">CloudStyle <span class="title">style_violet</span> <span class="params">(<span class="number">255.0</span>, <span class="number">0.0</span>, <span class="number">255.0</span>, <span class="number">8.0</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 全局变量</span></span><br><span class="line">std::string model_filename_;</span><br><span class="line">std::string scene_filename_;</span><br><span class="line"></span><br><span class="line"><span class="comment">// 算法参数</span></span><br><span class="line"><span class="function"><span class="type">bool</span> <span class="title">show_keypoints_</span> <span class="params">(<span class="literal">false</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">bool</span> <span class="title">use_hough_</span> <span class="params">(<span class="literal">true</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">model_ss_</span> <span class="params">(<span class="number">0.02f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">scene_ss_</span> <span class="params">(<span class="number">0.02f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">rf_rad_</span> <span class="params">(<span class="number">0.015f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">descr_rad_</span> <span class="params">(<span class="number">0.02f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">cg_size_</span> <span class="params">(<span class="number">0.01f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">cg_thresh_</span> <span class="params">(<span class="number">5.0f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">int</span> <span class="title">icp_max_iter_</span> <span class="params">(<span class="number">5</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">icp_corr_distance_</span> <span class="params">(<span class="number">0.005f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_clutter_reg_</span> <span class="params">(<span class="number">5.0f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_inlier_th_</span> <span class="params">(<span class="number">0.005f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_occlusion_th_</span> <span class="params">(<span class="number">0.01f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_rad_clutter_</span> <span class="params">(<span class="number">0.03f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_regularizer_</span> <span class="params">(<span class="number">3.0f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">float</span> <span class="title">hv_rad_normals_</span> <span class="params">(<span class="number">0.05f</span>)</span></span>;</span><br><span class="line"><span class="function"><span class="type">bool</span> <span class="title">hv_detect_clutter_</span> <span class="params">(<span class="literal">true</span>)</span></span>;</span><br><span class="line"></span><br><span class="line"><span class="comment">/**</span></span><br><span class="line"><span class="comment"> * 打印帮助信息</span></span><br><span class="line"><span class="comment"> * @param filename 可执行程序名称</span></span><br><span class="line"><span class="comment"> */</span></span><br><span class="line"><span class="function"><span class="type">void</span> <span class="title">showHelp</span> <span class="params">(<span class="type">char</span> *filename)</span></span></span><br><span class="line"><span class="function"></span>&#123;</span><br><span class="line">  std::cout &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;***************************************************************************&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;*                                                                         *&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;*          Global Hypothese Verification Tutorial - Usage Guide          *&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;*                                                                         *&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;***************************************************************************&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Usage: &quot;</span> &lt;&lt; filename &lt;&lt; <span class="string">&quot; model_filename.pcd scene_filename.pcd [Options]&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Options:&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     -h:                          Show this help.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     -k:                          Show keypoints.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --algorithm (Hough|GC):      Clustering algorithm used (default Hough).&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --model_ss val:              Model uniform sampling radius (default &quot;</span> &lt;&lt; model_ss_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --scene_ss val:              Scene uniform sampling radius (default &quot;</span> &lt;&lt; scene_ss_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --rf_rad val:                Reference frame radius (default &quot;</span> &lt;&lt; rf_rad_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --descr_rad val:             Descriptor radius (default &quot;</span> &lt;&lt; descr_rad_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --cg_size val:               Cluster size (default &quot;</span> &lt;&lt; cg_size_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --cg_thresh val:             Clustering threshold (default &quot;</span> &lt;&lt; cg_thresh_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --icp_max_iter val:          ICP max iterations number (default &quot;</span> &lt;&lt; icp_max_iter_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --icp_corr_distance val:     ICP correspondence distance (default &quot;</span> &lt;&lt; icp_corr_distance_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_clutter_reg val:        Clutter Regularizer (default &quot;</span> &lt;&lt; hv_clutter_reg_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_inlier_th val:          Inlier threshold (default &quot;</span> &lt;&lt; hv_inlier_th_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_occlusion_th val:       Occlusion threshold (default &quot;</span> &lt;&lt; hv_occlusion_th_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_rad_clutter val:        Clutter radius (default &quot;</span> &lt;&lt; hv_rad_clutter_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_regularizer val:        Regularizer value (default &quot;</span> &lt;&lt; hv_regularizer_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_rad_normals val:        Normals radius (default &quot;</span> &lt;&lt; hv_rad_normals_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;     --hv_detect_clutter val:     TRUE if clutter detect enabled (default &quot;</span> &lt;&lt; hv_detect_clutter_ &lt;&lt; <span class="string">&quot;)&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">&#125;</span><br><span class="line"></span><br><span class="line"><span class="comment">/**</span></span><br><span class="line"><span class="comment"> * 解析命令行参数</span></span><br><span class="line"><span class="comment"> * @param argc 参数个数</span></span><br><span class="line"><span class="comment"> * @param argv 参数数组</span></span><br><span class="line"><span class="comment"> */</span></span><br><span class="line"><span class="function"><span class="type">void</span> <span class="title">parseCommandLine</span> <span class="params">(<span class="type">int</span> argc, <span class="type">char</span> *argv[])</span></span></span><br><span class="line"><span class="function"></span>&#123;</span><br><span class="line">  <span class="keyword">if</span> (pcl::console::<span class="built_in">find_switch</span> (argc, argv, <span class="string">&quot;-h&quot;</span>))</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="built_in">showHelp</span> (argv[<span class="number">0</span>]);</span><br><span class="line">    <span class="built_in">exit</span> (<span class="number">0</span>);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  std::vector&lt;<span class="type">int</span>&gt; filenames = pcl::console::<span class="built_in">parse_file_extension_argument</span> (argc, argv, <span class="string">&quot;.pcd&quot;</span>);</span><br><span class="line">  <span class="keyword">if</span> (filenames.<span class="built_in">size</span> () != <span class="number">2</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;Filenames missing.\n&quot;</span>;</span><br><span class="line">    <span class="built_in">showHelp</span> (argv[<span class="number">0</span>]);</span><br><span class="line">    <span class="built_in">exit</span> (<span class="number">-1</span>);</span><br><span class="line">  &#125;</span><br><span class="line">  model_filename_ = argv[filenames[<span class="number">0</span>]];</span><br><span class="line">  scene_filename_ = argv[filenames[<span class="number">1</span>]];</span><br><span class="line"></span><br><span class="line">  <span class="keyword">if</span> (pcl::console::<span class="built_in">find_switch</span> (argc, argv, <span class="string">&quot;-k&quot;</span>))</span><br><span class="line">  &#123;</span><br><span class="line">    show_keypoints_ = <span class="literal">true</span>;</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  std::string used_algorithm;</span><br><span class="line">  <span class="keyword">if</span> (pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--algorithm&quot;</span>, used_algorithm) != <span class="number">-1</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="keyword">if</span> (used_algorithm == <span class="string">&quot;Hough&quot;</span>)</span><br><span class="line">    &#123;</span><br><span class="line">      use_hough_ = <span class="literal">true</span>;</span><br><span class="line">    &#125;</span><br><span class="line">    <span class="keyword">else</span> <span class="keyword">if</span> (used_algorithm == <span class="string">&quot;GC&quot;</span>)</span><br><span class="line">    &#123;</span><br><span class="line">      use_hough_ = <span class="literal">false</span>;</span><br><span class="line">    &#125;</span><br><span class="line">    <span class="keyword">else</span></span><br><span class="line">    &#123;</span><br><span class="line">      std::cout &lt;&lt; <span class="string">&quot;Wrong algorithm name.\n&quot;</span>;</span><br><span class="line">      <span class="built_in">showHelp</span> (argv[<span class="number">0</span>]);</span><br><span class="line">      <span class="built_in">exit</span> (<span class="number">-1</span>);</span><br><span class="line">    &#125;</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 解析其他参数...</span></span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--model_ss&quot;</span>, model_ss_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--scene_ss&quot;</span>, scene_ss_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--rf_rad&quot;</span>, rf_rad_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--descr_rad&quot;</span>, descr_rad_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--cg_size&quot;</span>, cg_size_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--cg_thresh&quot;</span>, cg_thresh_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--icp_max_iter&quot;</span>, icp_max_iter_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--icp_corr_distance&quot;</span>, icp_corr_distance_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_clutter_reg&quot;</span>, hv_clutter_reg_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_inlier_th&quot;</span>, hv_inlier_th_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_occlusion_th&quot;</span>, hv_occlusion_th_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_rad_clutter&quot;</span>, hv_rad_clutter_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_regularizer&quot;</span>, hv_regularizer_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_rad_normals&quot;</span>, hv_rad_normals_);</span><br><span class="line">  pcl::console::<span class="built_in">parse_argument</span> (argc, argv, <span class="string">&quot;--hv_detect_clutter&quot;</span>, hv_detect_clutter_);</span><br><span class="line">&#125;</span><br><span class="line"></span><br><span class="line"><span class="function"><span class="type">int</span> <span class="title">main</span> <span class="params">(<span class="type">int</span> argc, <span class="type">char</span> *argv[])</span></span></span><br><span class="line"><span class="function"></span>&#123;</span><br><span class="line">  <span class="built_in">parseCommandLine</span> (argc, argv);</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 创建点云指针</span></span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">model</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">model_keypoints</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">scene</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">scene_keypoints</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;NormalType&gt;::<span class="function">Ptr <span class="title">model_normals</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;NormalType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;NormalType&gt;::<span class="function">Ptr <span class="title">scene_normals</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;NormalType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;DescriptorType&gt;::<span class="function">Ptr <span class="title">model_descriptors</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;DescriptorType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;DescriptorType&gt;::<span class="function">Ptr <span class="title">scene_descriptors</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;DescriptorType&gt; ())</span></span>;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 加载点云</span></span><br><span class="line">  <span class="keyword">if</span> (pcl::io::<span class="built_in">loadPCDFile</span> (model_filename_, *model) &lt; <span class="number">0</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;Error loading model cloud.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    <span class="built_in">showHelp</span> (argv[<span class="number">0</span>]);</span><br><span class="line">    <span class="keyword">return</span> (<span class="number">-1</span>);</span><br><span class="line">  &#125;</span><br><span class="line">  <span class="keyword">if</span> (pcl::io::<span class="built_in">loadPCDFile</span> (scene_filename_, *scene) &lt; <span class="number">0</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;Error loading scene cloud.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    <span class="built_in">showHelp</span> (argv[<span class="number">0</span>]);</span><br><span class="line">    <span class="keyword">return</span> (<span class="number">-1</span>);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// ✅ 1. 在计算法向量之前，先清理模型和场景的原始点云</span></span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">model_clean</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt;)</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">scene_clean</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt;)</span></span>;</span><br><span class="line">  std::vector&lt;<span class="type">int</span>&gt; model_nan_indices, scene_nan_indices;</span><br><span class="line"></span><br><span class="line">  pcl::<span class="built_in">removeNaNFromPointCloud</span>(*model, *model_clean, model_nan_indices);</span><br><span class="line">  pcl::<span class="built_in">removeNaNFromPointCloud</span>(*scene, *scene_clean, scene_nan_indices);</span><br><span class="line"></span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Model: Removed &quot;</span> &lt;&lt; model_nan_indices.<span class="built_in">size</span>() &lt;&lt; <span class="string">&quot; NaN points.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Scene: Removed &quot;</span> &lt;&lt; scene_nan_indices.<span class="built_in">size</span>() &lt;&lt; <span class="string">&quot; NaN points.&quot;</span> &lt;&lt; std::endl;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// ✅ 2. 使用清理后的点云来计算法向量</span></span><br><span class="line">  pcl::NormalEstimationOMP&lt;PointType, NormalType&gt; norm_est;</span><br><span class="line">  norm_est.<span class="built_in">setKSearch</span> (<span class="number">10</span>);</span><br><span class="line">  norm_est.<span class="built_in">setInputCloud</span> (model_clean); <span class="comment">// 使用 model_clean</span></span><br><span class="line">  norm_est.<span class="built_in">compute</span> (*model_normals);</span><br><span class="line">  norm_est.<span class="built_in">setInputCloud</span> (scene_clean); <span class="comment">// 使用 scene_clean</span></span><br><span class="line">  norm_est.<span class="built_in">compute</span> (*scene_normals);</span><br><span class="line"></span><br><span class="line">  <span class="comment">// ✅ 3. 使用清理后的点云进行均匀采样</span></span><br><span class="line">  std::vector&lt;<span class="type">int</span>&gt; sampled_indices;</span><br><span class="line">  pcl::UniformSampling&lt;PointType&gt; uniform_sampling;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 对模型</span></span><br><span class="line">  uniform_sampling.<span class="built_in">setInputCloud</span>(model_clean); <span class="comment">// 使用 model_clean</span></span><br><span class="line">  uniform_sampling.<span class="built_in">setRadiusSearch</span>(model_ss_);</span><br><span class="line">  uniform_sampling.<span class="built_in">filter</span>(*model_keypoints); <span class="comment">// ✅ 直接输出到 model_keypoints 点云</span></span><br><span class="line">  <span class="comment">// 注意：我们不再需要索引，直接使用输出的点云</span></span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Model total points: &quot;</span> &lt;&lt; model_clean-&gt;<span class="built_in">size</span>() &lt;&lt; <span class="string">&quot;; Selected Keypoints: &quot;</span> &lt;&lt; model_keypoints-&gt;<span class="built_in">size</span>() &lt;&lt; std::endl;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 对场景</span></span><br><span class="line">  uniform_sampling.<span class="built_in">setInputCloud</span>(scene_clean); <span class="comment">// 使用 scene_clean</span></span><br><span class="line">  uniform_sampling.<span class="built_in">setRadiusSearch</span>(scene_ss_);</span><br><span class="line">  uniform_sampling.<span class="built_in">filter</span>(*scene_keypoints); <span class="comment">// ✅ 直接输出到 scene_keypoints 点云</span></span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Scene total points: &quot;</span> &lt;&lt; scene_clean-&gt;<span class="built_in">size</span>() &lt;&lt; <span class="string">&quot;; Selected Keypoints: &quot;</span> &lt;&lt; scene_keypoints-&gt;<span class="built_in">size</span>() &lt;&lt; std::endl;</span><br><span class="line"></span><br><span class="line"><span class="comment">/**</span></span><br><span class="line"><span class="comment"> *  Compute Descriptor for keypoints</span></span><br><span class="line"><span class="comment"> */</span></span><br><span class="line">pcl::SHOTEstimationOMP&lt;PointType, NormalType, DescriptorType&gt; descr_est;</span><br><span class="line">descr_est.<span class="built_in">setRadiusSearch</span> (descr_rad_);</span><br><span class="line"></span><br><span class="line"><span class="comment">// 为模型关键点计算描述子</span></span><br><span class="line">descr_est.<span class="built_in">setInputCloud</span> (model_keypoints);</span><br><span class="line">descr_est.<span class="built_in">setInputNormals</span> (model_normals);</span><br><span class="line"><span class="comment">// ✅ 使用清理后的 model_clean 作为搜索表面</span></span><br><span class="line">descr_est.<span class="built_in">setSearchSurface</span> (model_clean); </span><br><span class="line">descr_est.<span class="built_in">compute</span> (*model_descriptors);</span><br><span class="line"></span><br><span class="line"><span class="comment">// 为场景关键点计算描述子</span></span><br><span class="line">descr_est.<span class="built_in">setInputCloud</span> (scene_keypoints);</span><br><span class="line">descr_est.<span class="built_in">setInputNormals</span> (scene_normals);</span><br><span class="line"><span class="comment">// ✅ 使用清理后的 scene_clean 作为搜索表面</span></span><br><span class="line">descr_est.<span class="built_in">setSearchSurface</span> (scene_clean); </span><br><span class="line">descr_est.<span class="built_in">compute</span> (*scene_descriptors);</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 建立对应关系</span></span><br><span class="line">  <span class="function">pcl::CorrespondencesPtr <span class="title">model_scene_corrs</span> <span class="params">(<span class="keyword">new</span> pcl::Correspondences ())</span></span>;</span><br><span class="line">  pcl::KdTreeFLANN&lt;DescriptorType&gt; match_search;</span><br><span class="line">  match_search.<span class="built_in">setInputCloud</span> (model_descriptors);</span><br><span class="line"></span><br><span class="line">  std::vector&lt;<span class="type">int</span>&gt; model_good_keypoints_indices;</span><br><span class="line">  std::vector&lt;<span class="type">int</span>&gt; scene_good_keypoints_indices;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">for</span> (<span class="type">size_t</span> i = <span class="number">0</span>; i &lt; scene_descriptors-&gt;<span class="built_in">size</span> (); ++i)</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="function">std::vector&lt;<span class="type">int</span>&gt; <span class="title">neigh_indices</span> <span class="params">(<span class="number">1</span>)</span></span>;</span><br><span class="line">    <span class="function">std::vector&lt;<span class="type">float</span>&gt; <span class="title">neigh_sqr_dists</span> <span class="params">(<span class="number">1</span>)</span></span>;</span><br><span class="line">    <span class="keyword">if</span> (!std::<span class="built_in">isfinite</span>(scene_descriptors-&gt;<span class="built_in">at</span> (i).descriptor[<span class="number">0</span>])) <span class="comment">// 跳过 NaN</span></span><br><span class="line">    &#123;</span><br><span class="line">      <span class="keyword">continue</span>;</span><br><span class="line">    &#125;</span><br><span class="line">    <span class="type">int</span> found_neighs = match_search.<span class="built_in">nearestKSearch</span> (scene_descriptors-&gt;<span class="built_in">at</span> (i), <span class="number">1</span>, neigh_indices, neigh_sqr_dists);</span><br><span class="line">    <span class="keyword">if</span> (found_neighs == <span class="number">1</span> &amp;&amp; neigh_sqr_dists[<span class="number">0</span>] &lt; <span class="number">0.25f</span>)</span><br><span class="line">    &#123;</span><br><span class="line">      <span class="function">pcl::Correspondence <span class="title">corr</span> <span class="params">(neigh_indices[<span class="number">0</span>], <span class="keyword">static_cast</span>&lt;<span class="type">int</span>&gt;(i), neigh_sqr_dists[<span class="number">0</span>])</span></span>;</span><br><span class="line">      model_scene_corrs-&gt;<span class="built_in">push_back</span> (corr);</span><br><span class="line">      model_good_keypoints_indices.<span class="built_in">push_back</span> (corr.index_query);</span><br><span class="line">      scene_good_keypoints_indices.<span class="built_in">push_back</span> (corr.index_match);</span><br><span class="line">    &#125;</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">model_good_kp</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">scene_good_kp</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::<span class="built_in">copyPointCloud</span> (*model_keypoints, model_good_keypoints_indices, *model_good_kp);</span><br><span class="line">  pcl::<span class="built_in">copyPointCloud</span> (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;Correspondences found: &quot;</span> &lt;&lt; model_scene_corrs-&gt;<span class="built_in">size</span> () &lt;&lt; std::endl;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 聚类 (Hough 或 GC)</span></span><br><span class="line">  std::vector&lt;Eigen::Matrix4f, Eigen::aligned_allocator&lt;Eigen::Matrix4f&gt;&gt; rototranslations;</span><br><span class="line">  std::vector&lt;pcl::Correspondences&gt; clustered_corrs;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">if</span> (use_hough_)</span><br><span class="line">  &#123;</span><br><span class="line">    pcl::PointCloud&lt;RFType&gt;::<span class="function">Ptr <span class="title">model_rf</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;RFType&gt; ())</span></span>;</span><br><span class="line">    pcl::PointCloud&lt;RFType&gt;::<span class="function">Ptr <span class="title">scene_rf</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;RFType&gt; ())</span></span>;</span><br><span class="line">    pcl::BOARDLocalReferenceFrameEstimation&lt;PointType, NormalType, RFType&gt; rf_est;</span><br><span class="line">    rf_est.<span class="built_in">setFindHoles</span> (<span class="literal">true</span>);</span><br><span class="line">    rf_est.<span class="built_in">setRadiusSearch</span> (rf_rad_);</span><br><span class="line">    rf_est.<span class="built_in">setInputCloud</span> (model_keypoints);</span><br><span class="line">    rf_est.<span class="built_in">setInputNormals</span> (model_normals);</span><br><span class="line">    rf_est.<span class="built_in">setSearchSurface</span> (model);</span><br><span class="line">    rf_est.<span class="built_in">compute</span> (*model_rf);</span><br><span class="line"></span><br><span class="line">    rf_est.<span class="built_in">setInputCloud</span> (scene_keypoints);</span><br><span class="line">    rf_est.<span class="built_in">setInputNormals</span> (scene_normals);</span><br><span class="line">    rf_est.<span class="built_in">setSearchSurface</span> (scene);</span><br><span class="line">    rf_est.<span class="built_in">compute</span> (*scene_rf);</span><br><span class="line"></span><br><span class="line">    pcl::Hough3DGrouping&lt;PointType, PointType, RFType, RFType&gt; clusterer;</span><br><span class="line">    clusterer.<span class="built_in">setHoughBinSize</span> (cg_size_);</span><br><span class="line">    clusterer.<span class="built_in">setHoughThreshold</span> (cg_thresh_);</span><br><span class="line">    clusterer.<span class="built_in">setUseInterpolation</span> (<span class="literal">true</span>);</span><br><span class="line">    clusterer.<span class="built_in">setUseDistanceWeight</span> (<span class="literal">false</span>);</span><br><span class="line">    clusterer.<span class="built_in">setInputCloud</span> (model_keypoints);</span><br><span class="line">    clusterer.<span class="built_in">setInputRf</span> (model_rf);</span><br><span class="line">    clusterer.<span class="built_in">setSceneCloud</span> (scene_keypoints);</span><br><span class="line">    clusterer.<span class="built_in">setSceneRf</span> (scene_rf);</span><br><span class="line">    clusterer.<span class="built_in">setModelSceneCorrespondences</span> (model_scene_corrs);</span><br><span class="line">    clusterer.<span class="built_in">recognize</span> (rototranslations, clustered_corrs);</span><br><span class="line">  &#125;</span><br><span class="line">  <span class="keyword">else</span></span><br><span class="line">  &#123;</span><br><span class="line">    pcl::GeometricConsistencyGrouping&lt;PointType, PointType&gt; gc_clusterer;</span><br><span class="line">    gc_clusterer.<span class="built_in">setGCSize</span> (cg_size_);</span><br><span class="line">    gc_clusterer.<span class="built_in">setGCThreshold</span> (cg_thresh_);</span><br><span class="line">    gc_clusterer.<span class="built_in">setInputCloud</span> (model_keypoints);</span><br><span class="line">    gc_clusterer.<span class="built_in">setSceneCloud</span> (scene_keypoints);</span><br><span class="line">    gc_clusterer.<span class="built_in">setModelSceneCorrespondences</span> (model_scene_corrs);</span><br><span class="line">    gc_clusterer.<span class="built_in">recognize</span> (rototranslations, clustered_corrs);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">if</span> (rototranslations.<span class="built_in">size</span> () &lt;= <span class="number">0</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;*** No instances found! ***&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    <span class="keyword">return</span> (<span class="number">0</span>);</span><br><span class="line">  &#125;</span><br><span class="line">  <span class="keyword">else</span></span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;Recognized Instances: &quot;</span> &lt;&lt; rototranslations.<span class="built_in">size</span> () &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 生成每个实例的点云</span></span><br><span class="line">  std::vector&lt;pcl::PointCloud&lt;PointType&gt;::ConstPtr&gt; instances;</span><br><span class="line">  <span class="keyword">for</span> (<span class="type">size_t</span> i = <span class="number">0</span>; i &lt; rototranslations.<span class="built_in">size</span> (); ++i)</span><br><span class="line">  &#123;</span><br><span class="line">    pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">rotated_model</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">    pcl::<span class="built_in">transformPointCloud</span> (*model, *rotated_model, rototranslations[i]);</span><br><span class="line">    instances.<span class="built_in">push_back</span> (rotated_model);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// ICP 精配准</span></span><br><span class="line">  std::vector&lt;pcl::PointCloud&lt;PointType&gt;::ConstPtr&gt; registered_instances;</span><br><span class="line">  <span class="keyword">if</span> (<span class="literal">true</span>)</span><br><span class="line">  &#123;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;--- ICP ---------&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    <span class="keyword">for</span> (<span class="type">size_t</span> i = <span class="number">0</span>; i &lt; rototranslations.<span class="built_in">size</span> (); ++i)</span><br><span class="line">    &#123;</span><br><span class="line">      pcl::IterativeClosestPoint&lt;PointType, PointType&gt; icp;</span><br><span class="line">      icp.<span class="built_in">setMaximumIterations</span> (icp_max_iter_);</span><br><span class="line">      icp.<span class="built_in">setMaxCorrespondenceDistance</span> (icp_corr_distance_);</span><br><span class="line">      icp.<span class="built_in">setInputTarget</span> (scene);</span><br><span class="line">      icp.<span class="built_in">setInputSource</span> (instances[i]);</span><br><span class="line">      pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">registered</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt;)</span></span>;</span><br><span class="line">      icp.<span class="built_in">align</span> (*registered);</span><br><span class="line">      registered_instances.<span class="built_in">push_back</span> (registered);</span><br><span class="line">      std::cout &lt;&lt; <span class="string">&quot;Instance &quot;</span> &lt;&lt; i &lt;&lt; <span class="string">&quot; &quot;</span>;</span><br><span class="line">      <span class="keyword">if</span> (icp.<span class="built_in">hasConverged</span> ())</span><br><span class="line">      &#123;</span><br><span class="line">        std::cout &lt;&lt; <span class="string">&quot;Aligned!&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">      &#125;</span><br><span class="line">      <span class="keyword">else</span></span><br><span class="line">      &#123;</span><br><span class="line">        std::cout &lt;&lt; <span class="string">&quot;Not Aligned!&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">      &#125;</span><br><span class="line">    &#125;</span><br><span class="line">    std::cout &lt;&lt; <span class="string">&quot;-----------------&quot;</span> &lt;&lt; std::endl &lt;&lt; std::endl;</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 假设验证</span></span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;--- Hypotheses Verification ---&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">  std::vector&lt;<span class="type">bool</span>&gt; hypotheses_mask;</span><br><span class="line">  pcl::GlobalHypothesesVerification&lt;PointType, PointType&gt; GoHv;</span><br><span class="line">  GoHv.<span class="built_in">setSceneCloud</span> (scene);</span><br><span class="line">  GoHv.<span class="built_in">addModels</span> (registered_instances, <span class="literal">true</span>);</span><br><span class="line">  GoHv.<span class="built_in">setInlierThreshold</span> (hv_inlier_th_);</span><br><span class="line">  GoHv.<span class="built_in">setOcclusionThreshold</span> (hv_occlusion_th_);</span><br><span class="line">  GoHv.<span class="built_in">setRegularizer</span> (hv_regularizer_);</span><br><span class="line">  GoHv.<span class="built_in">setRadiusClutter</span> (hv_rad_clutter_);</span><br><span class="line">  GoHv.<span class="built_in">setClutterRegularizer</span> (hv_clutter_reg_);</span><br><span class="line">  GoHv.<span class="built_in">setDetectClutter</span> (hv_detect_clutter_);</span><br><span class="line">  GoHv.<span class="built_in">setRadiusNormals</span> (hv_rad_normals_);</span><br><span class="line">  GoHv.<span class="built_in">verify</span> ();</span><br><span class="line">  GoHv.<span class="built_in">getMask</span> (hypotheses_mask);</span><br><span class="line"></span><br><span class="line">  <span class="keyword">for</span> (<span class="type">int</span> i = <span class="number">0</span>; i &lt; <span class="built_in">static_cast</span>&lt;<span class="type">int</span>&gt;(hypotheses_mask.<span class="built_in">size</span> ()); i++)</span><br><span class="line">  &#123;</span><br><span class="line">    <span class="keyword">if</span> (hypotheses_mask[i])</span><br><span class="line">    &#123;</span><br><span class="line">      std::cout &lt;&lt; <span class="string">&quot;Instance &quot;</span> &lt;&lt; i &lt;&lt; <span class="string">&quot; is GOOD! &lt;---&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    &#125;</span><br><span class="line">    <span class="keyword">else</span></span><br><span class="line">    &#123;</span><br><span class="line">      std::cout &lt;&lt; <span class="string">&quot;Instance &quot;</span> &lt;&lt; i &lt;&lt; <span class="string">&quot; is bad!&quot;</span> &lt;&lt; std::endl;</span><br><span class="line">    &#125;</span><br><span class="line">  &#125;</span><br><span class="line">  std::cout &lt;&lt; <span class="string">&quot;-------------------------------&quot;</span> &lt;&lt; std::endl;</span><br><span class="line"></span><br><span class="line">  <span class="comment">// 可视化</span></span><br><span class="line">  pcl::<span class="function">visualization::PCLVisualizer <span class="title">viewer</span> <span class="params">(<span class="string">&quot;Global Hypotheses Verification&quot;</span>)</span></span>;</span><br><span class="line">  viewer.<span class="built_in">setBackgroundColor</span>(<span class="number">255</span>, <span class="number">255</span>, <span class="number">255</span>);</span><br><span class="line">  viewer.<span class="built_in">addPointCloud</span> (scene, <span class="string">&quot;scene_cloud&quot;</span>);</span><br><span class="line"></span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">off_scene_model</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">off_scene_model_keypoints</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line">  pcl::PointCloud&lt;PointType&gt;::<span class="function">Ptr <span class="title">off_model_good_kp</span> <span class="params">(<span class="keyword">new</span> pcl::PointCloud&lt;PointType&gt; ())</span></span>;</span><br><span class="line"></span><br><span class="line">  pcl::<span class="built_in">transformPointCloud</span> (*model, *off_scene_model, Eigen::<span class="built_in">Vector3f</span> (<span class="number">-1</span>, <span class="number">0</span>, <span class="number">0</span>), Eigen::<span class="built_in">Quaternionf</span> (<span class="number">1</span>, <span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>));</span><br><span class="line">  pcl::<span class="built_in">transformPointCloud</span> (*model_keypoints, *off_scene_model_keypoints, Eigen::<span class="built_in">Vector3f</span> (<span class="number">-1</span>, <span class="number">0</span>, <span class="number">0</span>), Eigen::<span class="built_in">Quaternionf</span> (<span class="number">1</span>, <span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>));</span><br><span class="line">  pcl::<span class="built_in">transformPointCloud</span> (*model_good_kp, *off_model_good_kp, Eigen::<span class="built_in">Vector3f</span> (<span class="number">-1</span>, <span class="number">0</span>, <span class="number">0</span>), Eigen::<span class="built_in">Quaternionf</span> (<span class="number">1</span>, <span class="number">0</span>, <span class="number">0</span>, <span class="number">0</span>));</span><br><span class="line"></span><br><span class="line">  <span class="keyword">if</span> (show_keypoints_)</span><br><span class="line">  &#123;</span><br><span class="line">    pcl::<span class="function">visualization::PointCloudColorHandlerCustom&lt;PointType&gt; <span class="title">off_scene_model_color_handler</span> <span class="params">(off_scene_model, style_white.r, style_white.g, style_white.b)</span></span>;</span><br><span class="line">    viewer.<span class="built_in">addPointCloud</span> (off_scene_model, off_scene_model_color_handler, <span class="string">&quot;off_scene_model&quot;</span>);</span><br><span class="line">    viewer.<span class="built_in">setPointCloudRenderingProperties</span> (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, style_white.size, <span class="string">&quot;off_scene_model&quot;</span>);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">if</span> (show_keypoints_)</span><br><span class="line">  &#123;</span><br><span class="line">    pcl::<span class="function">visualization::PointCloudColorHandlerCustom&lt;PointType&gt; <span class="title">model_good_keypoints_color_handler</span> <span class="params">(off_model_good_kp, style_violet.r, style_violet.g, style_violet.b)</span></span>;</span><br><span class="line">    viewer.<span class="built_in">addPointCloud</span> (off_model_good_kp, model_good_keypoints_color_handler, <span class="string">&quot;model_good_keypoints&quot;</span>);</span><br><span class="line">    viewer.<span class="built_in">setPointCloudRenderingProperties</span> (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, style_violet.size, <span class="string">&quot;model_good_keypoints&quot;</span>);</span><br><span class="line"></span><br><span class="line">    pcl::<span class="function">visualization::PointCloudColorHandlerCustom&lt;PointType&gt; <span class="title">scene_good_keypoints_color_handler</span> <span class="params">(scene_good_kp, style_violet.r, style_violet.g, style_violet.b)</span></span>;</span><br><span class="line">    viewer.<span class="built_in">addPointCloud</span> (scene_good_kp, scene_good_keypoints_color_handler, <span class="string">&quot;scene_good_keypoints&quot;</span>);</span><br><span class="line">    viewer.<span class="built_in">setPointCloudRenderingProperties</span> (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, style_violet.size, <span class="string">&quot;scene_good_keypoints&quot;</span>);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">for</span> (<span class="type">size_t</span> i = <span class="number">0</span>; i &lt; instances.<span class="built_in">size</span> (); ++i)</span><br><span class="line">  &#123;</span><br><span class="line">    std::stringstream ss_instance;</span><br><span class="line">    ss_instance &lt;&lt; <span class="string">&quot;instance_&quot;</span> &lt;&lt; i;</span><br><span class="line">    pcl::<span class="function">visualization::PointCloudColorHandlerCustom&lt;PointType&gt; <span class="title">instance_color_handler</span> <span class="params">(instances[i], style_red.r, style_red.g, style_red.b)</span></span>;</span><br><span class="line">    viewer.<span class="built_in">addPointCloud</span> (instances[i], instance_color_handler, ss_instance.<span class="built_in">str</span> ());</span><br><span class="line">    viewer.<span class="built_in">setPointCloudRenderingProperties</span> (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, style_red.size, ss_instance.<span class="built_in">str</span> ());</span><br><span class="line"></span><br><span class="line">    CloudStyle registeredStyle = hypotheses_mask[i] ? style_green : style_cyan;</span><br><span class="line">    ss_instance &lt;&lt; <span class="string">&quot;_registered&quot;</span>;</span><br><span class="line">    pcl::<span class="function">visualization::PointCloudColorHandlerCustom&lt;PointType&gt; <span class="title">registered_instance_color_handler</span> <span class="params">(registered_instances[i], registeredStyle.r, registeredStyle.g, registeredStyle.b)</span></span>;</span><br><span class="line">    viewer.<span class="built_in">addPointCloud</span> (registered_instances[i], registered_instance_color_handler, ss_instance.<span class="built_in">str</span> ());</span><br><span class="line">    viewer.<span class="built_in">setPointCloudRenderingProperties</span> (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyle.size, ss_instance.<span class="built_in">str</span> ());</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">while</span> (!viewer.<span class="built_in">wasStopped</span> ())</span><br><span class="line">  &#123;</span><br><span class="line">    viewer.<span class="built_in">spinOnce</span> (<span class="number">100</span>);</span><br><span class="line">  &#125;</span><br><span class="line"></span><br><span class="line">  <span class="keyword">return</span> (<span class="number">0</span>);</span><br><span class="line">&#125;</span><br></pre></td></tr></table></figure>

<p><img src="https://ckyfi9zero.github.io/picx-images-hosting/20250803/image.2dp0cwt4da.webp" alt="image"></p>
</article><div class="post-copyright"><div class="post-copyright__author"><span class="post-copyright-meta"><i class="fas fa-circle-user fa-fw"></i>文章作者: </span><span class="post-copyright-info"><a href="https://ckyfi9zero.github.io">Fi9zero</a></span></div><div class="post-copyright__type"><span class="post-copyright-meta"><i class="fas fa-square-arrow-up-right fa-fw"></i>文章链接: </span><span class="post-copyright-info"><a href="https://ckyfi9zero.github.io/2025/08/03/2025-08-03-3D%E5%AF%B9%E8%B1%A1%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81/">https://ckyfi9zero.github.io/2025/08/03/2025-08-03-3D%E5%AF%B9%E8%B1%A1%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81/</a></span></div><div class="post-copyright__notice"><span class="post-copyright-meta"><i class="fas fa-circle-exclamation fa-fw"></i>版权声明: </span><span class="post-copyright-info">本博客所有文章除特别声明外，均采用 <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/" target="_blank">CC BY-NC-SA 4.0</a> 许可协议。转载请注明来源 <a href="https://ckyfi9zero.github.io" target="_blank">荒岛</a>！</span></div></div><div class="tag_share"><div class="post-meta__tag-list"><a class="post-meta__tags" href="/tags/c/">c++</a><a class="post-meta__tags" href="/tags/PCL/">PCL</a></div><div class="post-share"><div class="social-share" data-image="/image/avatar.jpg" data-sites="facebook,twitter,wechat,weibo,qq"></div><link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/butterfly-extsrc/sharejs/dist/css/share.min.css" media="print" onload="this.media='all'"><script src="https://cdn.jsdelivr.net/npm/butterfly-extsrc/sharejs/dist/js/social-share.min.js" defer></script></div></div><nav class="pagination-post" id="pagination"><a class="pagination-related  no-desc" href="/2025/08/03/2025-08-03-%E9%9A%90%E5%BC%8F%E5%BD%A2%E7%8A%B6%E6%A8%A1%E5%9E%8B%E6%96%B9%E6%B3%95/" title="隐式形状模型方法"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info"><div class="info-1"><div class="info-item-1">上一篇</div><div class="info-item-2">隐式形状模型方法</div></div></div></a><a class="pagination-related  no-desc" href="/2025/08/03/2025-08-03-%E6%B1%82%E7%82%B9%E4%BA%91%E6%95%B0%E6%8D%AE%E7%9A%84%E8%BE%B9%E7%95%8C%E7%82%B9%E4%BA%91/" title="求点云数据的边界点云"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-right"><div class="info-1"><div class="info-item-1">下一篇</div><div class="info-item-2">求点云数据的边界点云</div></div></div></a></nav><div class="relatedPosts"><div class="headline"><i class="fas fa-thumbs-up fa-fw"></i><span>相关推荐</span></div><div class="relatedPosts-list"><a class="pagination-related no-desc" href="/2025/07/31/2025-07-31-%E7%94%A8kdtree%E5%AE%9E%E7%8E%B0%E5%BF%AB%E9%80%9F%E9%A2%86%E5%9F%9F%E6%90%9C%E7%B4%A2/" title="用kdtree实现快速领域搜索"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-07-31</div><div class="info-item-2">用kdtree实现快速领域搜索</div></div></div></a><a class="pagination-related no-desc" href="/2025/07/31/2025-07-31-%E7%94%A8%E5%85%AB%E5%8F%89%E6%A0%91%E8%BF%9B%E8%A1%8C%E6%97%A0%E5%BA%8F%E7%82%B9%E4%BA%91%E6%95%B0%E6%8D%AE%E7%9A%84%E7%A9%BA%E9%97%B4%E5%8F%98%E5%8C%96%E6%A3%80%E6%B5%8B/" title="用八叉树进行无序点云数据的空间变化检测"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-07-31</div><div class="info-item-2">用八叉树进行无序点云数据的空间变化检测</div></div></div></a><a class="pagination-related no-desc" href="/2025/07/31/2025-07-31-%E7%94%A8%E7%9B%B4%E9%80%9A%E6%BB%A4%E6%B3%A2%E5%99%A8%E5%AF%B9%E7%82%B9%E4%BA%91%E8%BF%9B%E8%A1%8C%E6%BB%A4%E6%B3%A2%E5%A4%84%E7%90%86/" title="用直通滤波器对点云进行滤波处理"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-07-31</div><div class="info-item-2">用直通滤波器对点云进行滤波处理</div></div></div></a><a class="pagination-related no-desc" href="/2025/08/01/2025-08-01-%E4%BB%8E%E7%82%B9%E4%BA%91%E5%88%9B%E5%BB%BA%E4%B8%80%E4%B8%AA%E6%B7%B1%E5%BA%A6%E5%9B%BE%E5%83%8F/" title="从点云创建一个深度图像"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-08-01</div><div class="info-item-2">从点云创建一个深度图像</div></div></div></a><a class="pagination-related no-desc" href="/2025/08/01/2025-08-01-%E4%BB%8E%E4%B8%80%E4%B8%AA%E7%82%B9%E4%BA%91%E4%B8%AD%E6%8F%90%E5%8F%96%E4%B8%80%E4%B8%AA%E5%AD%90%E9%9B%86/" title="从一个点云中提取一个子集"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-08-01</div><div class="info-item-2">从一个点云中提取一个子集</div></div></div></a><a class="pagination-related no-desc" href="/2025/08/01/2025-08-01-%E5%9C%A8%E6%BB%A4%E6%B3%A2%E5%99%A8%E6%A8%A1%E5%9D%97%E7%A7%BB%E9%99%A4%E7%A6%BB%E7%BE%A4%E7%82%B9/" title="在滤波器模块移除离群点"><div class="cover" style="background: var(--default-bg-color)"></div><div class="info text-center"><div class="info-1"><div class="info-item-1"><i class="far fa-calendar-alt fa-fw"></i> 2025-08-01</div><div class="info-item-2">在滤波器模块移除离群点</div></div></div></a></div></div></div><div class="aside-content" id="aside-content"><div class="card-widget card-info text-center"><div class="avatar-img"><img src="/image/avatar.jpg" onerror="this.onerror=null;this.src='/img/friend_404.gif'" alt="avatar"/></div><div class="author-info-name">Fi9zero</div><div class="author-info-description"></div><div class="site-data"><a href="/archives/"><div class="headline">文章</div><div class="length-num">54</div></a><a href="/tags/"><div class="headline">标签</div><div class="length-num">15</div></a><a href="/categories/"><div class="headline">分类</div><div class="length-num">8</div></a></div><a id="card-info-btn" target="_blank" rel="noopener" href="https://github.com/ckyFi9zero"><i class="fab fa-github"></i><span>Follow Me</span></a><div class="card-info-social-icons"><a class="social-icon" href="https://github.com/ckyFi9zero" target="_blank" title="Github"><i class="fab fa-github"></i></a><a class="social-icon" href="mailto:2539894206@qq.com" target="_blank" title="Email"><i class="fas fa-envelope"></i></a></div></div><div class="card-widget card-announcement"><div class="item-headline"><i class="fas fa-bullhorn fa-shake"></i><span>公告</span></div><div class="announcement_content">Blog</div></div><div class="sticky_layout"><div class="card-widget" id="card-toc"><div class="item-headline"><i class="fas fa-stream"></i><span>目录</span><span class="toc-percentage"></span></div><div class="toc-content"><ol class="toc"><li class="toc-item toc-level-1"><a class="toc-link" href="#%E5%9F%BA%E4%BA%8E%E5%85%A8%E5%B1%80%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81%E7%9A%84-3D-%E7%89%A9%E4%BD%93%E8%AF%86%E5%88%AB"><span class="toc-number">1.</span> <span class="toc-text">基于全局假设验证的 3D 物体识别</span></a><ol class="toc-child"><li class="toc-item toc-level-2"><a class="toc-link" href="#1-%E7%AE%97%E6%B3%95%E6%B5%81%E7%A8%8B%E6%80%BB%E8%A7%88"><span class="toc-number">1.1.</span> <span class="toc-text">1. 算法流程总览</span></a></li><li class="toc-item toc-level-2"><a class="toc-link" href="#2-%E6%A0%B8%E5%BF%83%E7%BB%84%E4%BB%B6%E8%AF%A6%E8%A7%A3"><span class="toc-number">1.2.</span> <span class="toc-text">2. 核心组件详解</span></a><ol class="toc-child"><li class="toc-item toc-level-3"><a class="toc-link" href="#2-1-%E9%A2%84%E5%A4%84%E7%90%86%EF%BC%9AremoveNaNFromPointCloud"><span class="toc-number">1.2.1.</span> <span class="toc-text">2.1 预处理：removeNaNFromPointCloud</span></a></li><li class="toc-item toc-level-3"><a class="toc-link" href="#2-2-%E5%85%B3%E9%94%AE%E7%82%B9%E6%8F%90%E5%8F%96%EF%BC%9AUniformSampling"><span class="toc-number">1.2.2.</span> <span class="toc-text">2.2 关键点提取：UniformSampling</span></a></li><li class="toc-item toc-level-3"><a class="toc-link" href="#2-3-%E7%89%B9%E5%BE%81%E6%8F%8F%E8%BF%B0%EF%BC%9ASHOTEstimationOMP"><span class="toc-number">1.2.3.</span> <span class="toc-text">2.3 特征描述：SHOTEstimationOMP</span></a></li><li class="toc-item toc-level-3"><a class="toc-link" href="#2-4-%E5%A7%BF%E6%80%81%E8%81%9A%E7%B1%BB%EF%BC%9AHough3DGrouping-vs-GeometricConsistencyGrouping"><span class="toc-number">1.2.4.</span> <span class="toc-text">2.4 姿态聚类：Hough3DGrouping vs GeometricConsistencyGrouping</span></a></li><li class="toc-item toc-level-3"><a class="toc-link" href="#2-5-%E7%B2%BE%E9%85%8D%E5%87%86%EF%BC%9AIterativeClosestPoint-ICP"><span class="toc-number">1.2.5.</span> <span class="toc-text">2.5 精配准：IterativeClosestPoint (ICP)</span></a></li><li class="toc-item toc-level-3"><a class="toc-link" href="#2-6-%E5%81%87%E8%AE%BE%E9%AA%8C%E8%AF%81%EF%BC%9AGlobalHypothesesVerification"><span class="toc-number">1.2.6.</span> <span class="toc-text">2.6 假设验证：GlobalHypothesesVerification</span></a></li></ol></li><li class="toc-item toc-level-2"><a class="toc-link" href="#3-%E5%85%B3%E9%94%AE%E4%BF%AE%E5%A4%8D%E7%82%B9"><span class="toc-number">1.3.</span> <span class="toc-text">3. 关键修复点</span></a></li><li class="toc-item toc-level-2"><a class="toc-link" href="#4-%E5%8F%AF%E8%A7%86%E5%8C%96%E5%86%85%E5%AE%B9"><span class="toc-number">1.4.</span> <span class="toc-text">4. 可视化内容</span></a></li><li class="toc-item toc-level-2"><a class="toc-link" href="#5-%E8%BF%90%E8%A1%8C%E5%91%BD%E4%BB%A4"><span class="toc-number">1.5.</span> <span class="toc-text">5. 运行命令</span></a></li><li class="toc-item toc-level-2"><a class="toc-link" href="#%E4%BB%A3%E7%A0%81%E5%AE%9E%E7%8E%B0"><span class="toc-number">1.6.</span> <span class="toc-text">代码实现</span></a></li></ol></li></ol></div></div><div class="card-widget card-recent-post"><div class="item-headline"><i class="fas fa-history"></i><span>最新文章</span></div><div class="aside-list"><div class="aside-list-item no-cover"><div class="content"><a class="title" href="/2025/10/05/2025-10-05-lio_sam/" title="lio_sam">lio_sam</a><time datetime="2025-10-05T02:43:51.000Z" title="发表于 2025-10-05 10:43:51">2025-10-05</time></div></div><div class="aside-list-item no-cover"><div class="content"><a class="title" href="/2025/08/12/2025-08-12-%E4%BD%BF%E7%94%A8docker%E6%90%AD%E5%BB%BApytorch%E7%8E%AF%E5%A2%83/" title="使用docker搭建pytorch环境">使用docker搭建pytorch环境</a><time datetime="2025-08-12T12:43:33.000Z" title="发表于 2025-08-12 20:43:33">2025-08-12</time></div></div><div class="aside-list-item no-cover"><div class="content"><a class="title" href="/2025/08/05/2025-08-05-docker/" title="docker">docker</a><time datetime="2025-08-05T11:02:36.000Z" title="发表于 2025-08-05 19:02:36">2025-08-05</time></div></div><div class="aside-list-item no-cover"><div class="content"><a class="title" href="/2025/08/05/2025-08-05-%E8%BF%90%E5%8A%A8%E5%AF%B9%E8%B1%A1%E5%88%86%E5%89%B2%E4%B8%8E%E9%85%8D%E5%87%86%E7%AE%97%E6%B3%95/" title="运动对象分割与配准算法">运动对象分割与配准算法</a><time datetime="2025-08-05T08:16:02.000Z" title="发表于 2025-08-05 16:16:02">2025-08-05</time></div></div><div class="aside-list-item no-cover"><div class="content"><a class="title" href="/2025/08/05/2025-08-05-%E6%9D%A1%E4%BB%B6%E6%AC%A7%E5%BC%8F%E8%81%9A%E7%B1%BB%E5%88%86%E5%89%B2/" title="条件欧式聚类分割">条件欧式聚类分割</a><time datetime="2025-08-05T07:56:39.000Z" title="发表于 2025-08-05 15:56:39">2025-08-05</time></div></div></div></div></div></div></main><footer id="footer"><div class="footer-other"><div class="footer-copyright"><span class="copyright">&copy;&nbsp;2025 By Fi9zero</span><span class="framework-info"><span>框架 </span><a target="_blank" rel="noopener" href="https://hexo.io">Hexo 7.3.0</a><span class="footer-separator">|</span><span>主题 </span><a target="_blank" rel="noopener" href="https://github.com/jerryc127/hexo-theme-butterfly">Butterfly 5.4.2</a></span></div><div class="footer_custom_text">Only light can attract bugs.</div></div></footer></div><div id="rightside"><div id="rightside-config-hide"><button id="readmode" type="button" title="阅读模式"><i class="fas fa-book-open"></i></button><button id="darkmode" type="button" title="日间和夜间模式切换"><i class="fas fa-adjust"></i></button><button id="hide-aside-btn" type="button" title="单栏和双栏切换"><i class="fas fa-arrows-alt-h"></i></button></div><div id="rightside-config-show"><button id="rightside-config" type="button" title="设置"><i class="fas fa-cog fa-spin"></i></button><button class="close" id="mobile-toc-button" type="button" title="目录"><i class="fas fa-list-ul"></i></button><button id="go-up" type="button" title="回到顶部"><span class="scroll-percent"></span><i class="fas fa-arrow-up"></i></button></div></div><div><script src="/js/utils.js"></script><script src="/js/main.js"></script><div class="js-pjax"><script>(() => {
  const loadMathjax = () => {
    if (!window.MathJax) {
      window.MathJax = {
        tex: {
          inlineMath: [['$', '$'], ['\\(', '\\)']],
          tags: 'none',
        },
        chtml: {
          scale: 1.1
        },
        options: {
          enableMenu: true,
          renderActions: {
            findScript: [10, doc => {
              for (const node of document.querySelectorAll('script[type^="math/tex"]')) {
                const display = !!node.type.match(/; *mode=display/)
                const math = new doc.options.MathItem(node.textContent, doc.inputJax[0], display)
                const text = document.createTextNode('')
                node.parentNode.replaceChild(text, node)
                math.start = {node: text, delim: '', n: 0}
                math.end = {node: text, delim: '', n: 0}
                doc.math.push(math)
              }
            }, '']
          }
        }
      }

      const script = document.createElement('script')
      script.src = 'https://cdn.jsdelivr.net/npm/mathjax/es5/tex-mml-chtml.min.js'
      script.id = 'MathJax-script'
      script.async = true
      document.head.appendChild(script)
    } else {
      MathJax.startup.document.state(0)
      MathJax.texReset()
      MathJax.typesetPromise()
    }
  }

  btf.addGlobalFn('encrypt', loadMathjax, 'mathjax')
  window.pjax ? loadMathjax() : window.addEventListener('load', loadMathjax)
})()</script><script>(() => {
  const runMermaid = ele => {
    window.loadMermaid = true
    const theme = document.documentElement.getAttribute('data-theme') === 'dark' ? 'dark' : 'default'

    ele.forEach((item, index) => {
      const mermaidSrc = item.firstElementChild
      const mermaidThemeConfig = `%%{init:{ 'theme':'${theme}'}}%%\n`
      const mermaidID = `mermaid-${index}`
      const mermaidDefinition = mermaidThemeConfig + mermaidSrc.textContent

      const renderFn = mermaid.render(mermaidID, mermaidDefinition)
      const renderMermaid = svg => {
        mermaidSrc.insertAdjacentHTML('afterend', svg)
      }

      // mermaid v9 and v10 compatibility
      typeof renderFn === 'string' ? renderMermaid(renderFn) : renderFn.then(({ svg }) => renderMermaid(svg))
    })
  }

  const codeToMermaid = () => {
    const codeMermaidEle = document.querySelectorAll('pre > code.mermaid')
    if (codeMermaidEle.length === 0) return

    codeMermaidEle.forEach(ele => {
      const preEle = document.createElement('pre')
      preEle.className = 'mermaid-src'
      preEle.hidden = true
      preEle.textContent = ele.textContent
      const newEle = document.createElement('div')
      newEle.className = 'mermaid-wrap'
      newEle.appendChild(preEle)
      ele.parentNode.replaceWith(newEle)
    })
  }

  const loadMermaid = () => {
    if (true) codeToMermaid()
    const $mermaid = document.querySelectorAll('#article-container .mermaid-wrap')
    if ($mermaid.length === 0) return

    const runMermaidFn = () => runMermaid($mermaid)
    btf.addGlobalFn('themeChange', runMermaidFn, 'mermaid')
    window.loadMermaid ? runMermaidFn() : btf.getScript('https://cdn.jsdelivr.net/npm/mermaid/dist/mermaid.min.js').then(runMermaidFn)
  }

  btf.addGlobalFn('encrypt', loadMermaid, 'mermaid')
  window.pjax ? loadMermaid() : document.addEventListener('DOMContentLoaded', loadMermaid)
})()</script></div><script defer src="https://npm.elemecdn.com/jquery@latest/dist/jquery.min.js"></script><script defer data-pjax src="/js/pig.js"></script><script src="/js/weather.js"></script><script async data-pjax src="//busuanzi.ibruce.info/busuanzi/2.3/busuanzi.pure.mini.js"></script><div id="local-search"><div class="search-dialog"><nav class="search-nav"><span class="search-dialog-title">搜索</span><span id="loading-status"></span><button class="search-close-button"><i class="fas fa-times"></i></button></nav><div class="text-center" id="loading-database"><i class="fas fa-spinner fa-pulse"></i><span>  数据加载中</span></div><div class="search-wrap"><div id="local-search-input"><div class="local-search-box"><input class="local-search-box--input" placeholder="站内搜索..." type="text"/></div></div><hr/><div id="local-search-results"></div><div id="local-search-stats-wrap"></div></div></div><div id="search-mask"></div><script src="/js/search/local-search.js"></script></div></div></body></html>